#ifndef __VECTOR__
#define __VECTOR__

#include <iostream>
#include <limits>
#include <cmath>
#include <cassert>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/array.hpp>

namespace math {

template<class T, unsigned SIZE>
struct Vec {
	Vec();
	Vec(T data[SIZE]);
	explicit Vec(const T& t);
	
	template<class U>
	Vec(const Vec<U, SIZE>& u) {
		for(size_t i = 0; i < SIZE; i++)
			data[i] = static_cast<T>(u.data[i]);
	}

	size_t size() const {return SIZE;}

	T data[SIZE];

	Vec<T, SIZE>& operator=(const Vec<T, SIZE>& a);
	Vec<T, SIZE>& operator=(const T& a);
	Vec<T, SIZE>& operator+=(const Vec<T, SIZE>& a);
	Vec<T, SIZE>& operator-=(const Vec<T, SIZE>& a);

	Vec<T, SIZE>& MulAdd(const Vec<T, SIZE>& a, const T& t);
	Vec<T, SIZE>& MulAdd(const T& t, const Vec<T, SIZE>& a1, const Vec<T, SIZE>& a2);
	Vec<T, SIZE>& Lerp(const T& t1, const T& t2, const Vec<T, SIZE>& a1, const Vec<T, SIZE>& a2);
	
	void Set(T t0) {
		data[0] = t0;
	}
	
	void Set(T t0, T t1) {
		data[0] = t0;
		data[1] = t1;
	}
	
	void Set(T t0, T t1, T t2) {
		data[0] = t0;
		data[1] = t1;
		data[2] = t2;
	}

	void Set(T t0, T t1, T t2, T t3) {
		data[0] = t0;
		data[1] = t1;
		data[2] = t2;
		data[3] = t3;
	}
	
	void Set(T t0, T t1, T t2, T t3, T t4) {
		data[0] = t0;
		data[1] = t1;
		data[2] = t2;
		data[3] = t3;
		data[4] = t4;
	}
	
	void Set(T t0, T t1, T t2, T t3, T t4, T t5) {
		data[0] = t0;
		data[1] = t1;
		data[2] = t2;
		data[3] = t3;
		data[4] = t4;
		data[5] = t5;
	}
	
	void Set(T t0, T t1, T t2, T t3, T t4, T t5, T t6) {
		data[0] = t0;
		data[1] = t1;
		data[2] = t2;
		data[3] = t3;
		data[4] = t4;
		data[5] = t5;
		data[6] = t6;
	}
	
	void Set(T t0, T t1, T t2, T t3, T t4, T t5, T t6, T t7, T t8) {
		data[0] = t0;
		data[1] = t1;
		data[2] = t2;
		data[3] = t3;
		data[4] = t4;
		data[5] = t5;
		data[6] = t6;
		data[7] = t7;
		data[8] = t8;
	}
	
	void Set(T t0, T t1, T t2, T t3, T t4, T t5, T t6, T t7, T t8, T t9) {
		data[0] = t0;
		data[1] = t1;
		data[2] = t2;
		data[3] = t3;
		data[4] = t4;
		data[5] = t5;
		data[6] = t6;
		data[7] = t7;
		data[8] = t8;
		data[9] = t9;
	}
	
	void Set(T t0, T t1, T t2, T t3, T t4, T t5, T t6, T t7, T t8, T t9, T t10, T t11, T t12) {
		data[0] = t0;
		data[1] = t1;
		data[2] = t2;
		data[3] = t3;
		data[4] = t4;
		data[5] = t5;
		data[6] = t6;
		data[7] = t7;
		data[8] = t8;
		data[9] = t9;
		data[10] = t10;
		data[11] = t11;
		data[12] = t12;
	}
	
	void Set(T t0, T t1, T t2, T t3, T t4, T t5, T t6, T t7, T t8, T t9, T t10, T t11, T t12, T t13, T t14) {
		data[0] = t0;
		data[1] = t1;
		data[2] = t2;
		data[3] = t3;
		data[4] = t4;
		data[5] = t5;
		data[6] = t6;
		data[7] = t7;
		data[8] = t8;
		data[9] = t9;
		data[10] = t10;
		data[11] = t11;
		data[12] = t12;
		data[13] = t13;
		data[14] = t14;
	}

	T GetMaxComponent() const {
		T temp = -std::numeric_limits<T>::max();
		for(size_t i = 0; i < SIZE; i++) {
			if(data[i] > temp)
				temp = data[i];
		}
		return temp;
	}

	T GetMinComponent() const {
		T temp = std::numeric_limits<T>::max();
		for(size_t i = 0; i < SIZE; i++) {
			if(data[i] < temp)
				temp = data[i];
		}
		return temp;
	}

	T& operator[](size_t index);
	const T& operator[](size_t index) const;

	Vec<T, SIZE - 1> ReduceDim() const;
};

template<class T>
inline Vec<T, 1> V1n(T x) {
	Vec<T, 1> v;
	v.Set(x);
	return v;
}

inline Vec<float, 1> V1f(float x) {
	Vec<float, 1> v;
	v.Set(x);
	return v;
}

inline Vec<float, 2> V2f(float x, float y) {
	Vec<float, 2> v;
	v.Set(x, y);
	return v;
}

inline Vec<double, 2> V2d(double x, double y) {
	Vec<double, 2> v;
	v.Set(x, y);
	return v;
}

inline Vec<float, 3> V3f(float x, float y, float z) {
	Vec<float, 3> v;
	v.Set(x, y, z);
	return v;
}

inline Vec<double, 3> V3d(double x, double y, double z) {
	Vec<double, 3> v;
	v.Set(x, y, z);
	return v;
}

inline Vec<float, 4> V4f(float x, float y, float z, float w) {
	Vec<float, 4> v;
	v.Set(x, y, z, w);
	return v;
}

inline Vec<float, 4> V4f(const Vec<float, 3>& u, float w) {
	Vec<float, 4> v;
	v.Set(u[0], u[1], u[2], w);
	return v;
}

inline Vec<double, 4> V4d(double x, double y, double z, double w) {
	Vec<double, 4> v;
	v.Set(x, y, z, w);
	return v;
}

inline Vec<int, 2> V2i(int x, int y) {
	Vec<int, 2> v;
	v.Set(x, y);
	return v;
}

inline Vec<size_t, 2> V2ui(size_t x, size_t y) {
	Vec<size_t, 2> v;
	v.Set(x, y);
	return v;
}

inline Vec<size_t, 3> V3ui(size_t x, size_t y, size_t z) {
	Vec<size_t, 3> v;
	v.Set(x, y, z);
	return v;
}

inline Vec<size_t, 4> V4ui(size_t x, size_t y, size_t z, size_t w) {
	Vec<size_t, 4> v;
	v.Set(x, y, z, w);
	return v;
}

template<class T>
inline Vec<T, 2> V2n(T x, T y) {
	Vec<T, 2> v;
	v.Set(x, y);
	return v;
}

template<class T>
inline Vec<T, 3> V3n(T x, T y, T z) {
	Vec<T, 3> v;
	v.Set(x, y, z);
	return v;
}

template<class T>
inline Vec<T, 3> V3n(const Vec<T, 2>& u, T z) {
	Vec<T, 3> v;
	v.Set(u[0], u[1], z);
	return v;
}

template<class T>
inline Vec<T, 4> V4n(T x, T y, T z, T w) {
	Vec<T, 4> v;
	v.Set(x, y, z, w);
	return v;
}

template<class T>
	inline Vec<T, 4> V4n(const Vec<T, 3>& u, T w) {
	Vec<T, 4> v;
	v.Set(u[0], u[1], u[2], w);
	return v;
}

template<class T>
	inline Vec<T, 4> V4n(const Vec<T, 2>& u, T z, T w) {
	Vec<T, 4> v;
	v.Set(u[0], u[1], z, w);
	return v;
}

template<class T>
inline Vec<T, 5> V5n(T x0, T x1, T x2, T x3, T x4) {
	Vec<T, 5> v;
	v.Set(x0, x1, x2, x3, x4);
	return v;
}

template<class T>
inline Vec<T, 6> V6n(T x0, T x1, T x2, T x3, T x4, T x5) {
	Vec<T, 6> v;
	v.Set(x0, x1, x2, x3, x4, x5);
	return v;
}

template<class T>
inline Vec<T, 7> V7n(T x0, T x1, T x2, T x3, T x4, T x5, T x6) {
	Vec<T, 7> v;
	v.Set(x0, x1, x2, x3, x4, x5, x6);
	return v;
}

template<class T>
inline Vec<T, 9> V9n(T x0, T x1, T x2, T x3, T x4, T x5, T x6, T x7, T x8) {
	Vec<T, 9> v;
	v.Set(x0, x1, x2, x3, x4, x5, x6, x7, x8);
	return v;
}

template<class T>
inline Vec<T, 10> V10n(T x0, T x1, T x2, T x3, T x4, T x5, T x6, T x7, T x8, T x9) {
	Vec<T, 10> v;
	v.Set(x0, x1, x2, x3, x4, x5, x6, x7, x8, x9);
	return v;
}

template<class T>
inline Vec<T, 13> V13n(T x0, T x1, T x2, T x3, T x4, T x5, T x6, T x7, T x8, T x9, T x10, T x11, T x12) {
	Vec<T, 13> v;
	v.Set(x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, x10, x11, x12);
	return v;
}

template<class T>
inline Vec<T, 15> V15n(T x0, T x1, T x2, T x3, T x4, T x5, T x6, T x7, T x8, T x9, T x10, T x11, T x12, T x13, T x14) {
	Vec<T, 15> v;
	v.Set(x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, x10, x11, x12, x13, x14);
	return v;
}

template<class T, unsigned SIZE>
Vec<T, SIZE> Min(const Vec<T, SIZE>& v1, const Vec<T, SIZE>& v2) {
	Vec<T, SIZE> v3;
	for(size_t i = 0; i < SIZE; i++) {
		v3.data[i] = v1.data[i] < v2.data[i] ? v1.data[i] : v2.data[i];
	}
	return v3;
}

template<class T, unsigned SIZE>
Vec<T, SIZE> Max(const Vec<T, SIZE>& v1, const Vec<T, SIZE>& v2) {
	Vec<T, SIZE> v3;
	for(size_t i = 0; i < SIZE; i++) {
		v3.data[i] = v1.data[i] > v2.data[i] ? v1.data[i] : v2.data[i];
	}
	return v3;
}

template<class T, unsigned SIZE>
Vec<T, SIZE> Invert(const Vec<T, SIZE>& v) {
	Vec<T, SIZE> u;
	for(size_t i = 0; i < SIZE; i++) {
		u.data[i] = 1.0f / v.data[i];
	}
	return u;
}

template<class T, unsigned SIZE>
Vec<T, SIZE> Pow(const Vec<T, SIZE>& v, const T& e) {
	Vec<T, SIZE> u;
	for(size_t i = 0; i < SIZE; i++) {
		u.data[i] = std::pow(v.data[i], e);
	}
	return u;
}

template<class T, unsigned SIZE>
Vec<T, SIZE> Sqrt(const Vec<T, SIZE>& v) {
	Vec<T, SIZE> u;
	for(size_t i = 0; i < SIZE; i++) {
		u.data[i] = std::sqrt(v.data[i]);
	}
	return u;
}

template<class T, unsigned SIZE>
Vec<T, SIZE> operator*(const Vec<T, SIZE>& v1, const Vec<T, SIZE>& v2) {
	Vec<T, SIZE> v3;
	for(size_t i = 0; i < SIZE; i++) {
		v3.data[i] = v1.data[i] * v2.data[i];
	}
	return v3;
}

template<class T, unsigned SIZE>
Vec<T, SIZE> operator/(const Vec<T, SIZE>& v1, const Vec<T, SIZE>& v2) {
	Vec<T, SIZE> v3;
	for(size_t i = 0; i < SIZE; i++) {
		v3.data[i] = v1.data[i] / v2.data[i];
	}
	return v3;
}

template<class T, unsigned SIZE>
bool operator==(const Vec<T, SIZE>& a, const Vec<T, SIZE>& b) {
  for(size_t i = 0; i < SIZE; ++i) {
    if(a.data[i] != b.data[i])
      return false;
  }
  return true;
}

template<class T, unsigned SIZE>
bool operator!=(const Vec<T, SIZE>& a, const Vec<T, SIZE>& b) {
  return !(a == b);
}

template<class T, unsigned SIZE>
Vec<T, SIZE>::Vec() {
  for(size_t i = 0; i < SIZE; ++i)
    data[i] = (T)0;
}

template<class T, unsigned SIZE>
Vec<T, SIZE>::Vec(T data[SIZE]) {
  for(size_t i = 0; i < SIZE; ++i)
    this->data[i] = data[i];
}

template<class T, unsigned SIZE>
Vec<T, SIZE>::Vec(const T& t) {
  for(size_t i = 0; i < SIZE; ++i)
	data[i] = t;
}

template<class T, unsigned SIZE>
boost::numeric::ublas::matrix<T> GetMatrix(const boost::array< Vec<T, SIZE>, SIZE >& a) {
	boost::numeric::ublas::matrix<T> m(SIZE, SIZE);
	for(size_t i = 0; i < SIZE; i++)
		for(size_t j = 0; j < SIZE; j++)
			m(i, j) = a[i][j];

	return m;
}

template<class T, unsigned SIZE>
Vec<T, SIZE>& Vec<T, SIZE>::operator=(const Vec<T, SIZE>& a) {
  if(this != &a) {
    for(size_t i = 0; i < SIZE; ++i)
      data[i] = a.data[i];
  }
  return *this;
}

template<class T, unsigned SIZE>
Vec<T, SIZE>& Vec<T, SIZE>::operator=(const T& a) {
  for(size_t i = 0; i < SIZE; ++i)
    data[i] = a;
  return *this;
}

template<class T, unsigned SIZE>
Vec<T, SIZE>& Vec<T, SIZE>::operator+=(const Vec<T, SIZE>& a) {
  for(size_t i = 0; i < SIZE; ++i)
      data[i] += a.data[i];
  return *this;
}
	
template<class T, unsigned SIZE>
Vec<T, SIZE>& Vec<T, SIZE>::MulAdd(const Vec<T, SIZE>& a, const T& t) {
  for(size_t i = 0; i < SIZE; ++i)
      data[i] += t * a.data[i];
  return *this;
}

template<class T, unsigned SIZE>
Vec<T, SIZE>& Vec<T, SIZE>::MulAdd(const T& t, const Vec<T, SIZE>& a1, const Vec<T, SIZE>& a2) {
	for(size_t i = 0; i < SIZE; ++i)
		data[i] = t * (a1[i] + a2[i]);
	return *this;
}

template<class T, unsigned SIZE>
Vec<T, SIZE>& Vec<T, SIZE>::Lerp(const T& t1, const T& t2, const Vec<T, SIZE>& a1, const Vec<T, SIZE>& a2) {
	for(size_t i = 0; i < SIZE; i++)
		data[i] = t1 * a1.data[i] + t2 * a2.data[i];
	return *this;
}

template<class T, unsigned SIZE>
Vec<T, SIZE>& Vec<T, SIZE>::operator-=(const Vec<T, SIZE>& a) {
  if(this != &a) {
    for(size_t i = 0; i < SIZE; ++i)
      data[i] -= a.data[i];
  }
  return *this;
}


template<class T, unsigned SIZE>
Vec<T, SIZE> operator-(const Vec<T, SIZE>& a, const Vec<T, SIZE>& b) {
  Vec<T, SIZE> r;
  for(size_t i = 0; i < SIZE; ++i)
    r.data[i] = a.data[i] - b.data[i];
  return r;
}

template<class T, unsigned SIZE>
Vec<T, SIZE> operator+(const Vec<T, SIZE>& a, const Vec<T, SIZE>& b) {
  Vec<T, SIZE> r;
  for(size_t i = 0; i < SIZE; ++i)
    r.data[i] = a.data[i] + b.data[i];
  return r;
}

template<class T, unsigned SIZE>
Vec<T, SIZE> operator*(const Vec<T, SIZE>& a, T s) {
  Vec<T, SIZE> r;
  for(size_t i = 0; i < SIZE; ++i)
    r.data[i] = a.data[i] * s;
  return r;
}

template<class T, unsigned SIZE>
Vec<T, SIZE> operator*(T s, const Vec<T, SIZE>& a) {
  Vec<T, SIZE> r;
  for(size_t i = 0; i < SIZE; ++i)
    r.data[i] = a.data[i] * s;
  return r;
}

template<class T, unsigned SIZE>
Vec<T, SIZE> operator/(const Vec<T, SIZE>& a, T s) {
  Vec<T, SIZE> r;
  for(size_t i = 0; i < SIZE; ++i)
    r.data[i] = a.data[i] / s;
  return r;
}

template<class T, unsigned SIZE>
T dot(const Vec<T, SIZE>& a, const Vec<T, SIZE>& b) {
  T t(0);
  for(size_t i = 0; i < SIZE; ++i)
    t += (a.data[i] * b.data[i]);
  return t;
}

template<class T, unsigned SIZE>
T length(const Vec<T, SIZE>& v) {
  T t(0);
  for(size_t i = 0; i < SIZE; ++i)
    t += (v.data[i] * v.data[i]);
  return std::sqrt(t);
}

template<class T, unsigned SIZE>
T lengthSquared(const Vec<T, SIZE>& v) {
  T t(0);
  for(size_t i = 0; i < SIZE; ++i)
    t += (v.data[i] * v.data[i]);
  return t;
}

template<class T, unsigned SIZE>
T& Vec<T, SIZE>::operator[](size_t index) {
  return data[index];
}

template<class T, unsigned SIZE>
const T& Vec<T, SIZE>::operator[](size_t index) const {
  return data[index];
}

template<class T, unsigned SIZE>
Vec<T, SIZE - 1> Vec<T, SIZE>::ReduceDim() const {
	Vec<T, SIZE - 1> P;
	for(size_t i = 0; i < SIZE - 1; i++)
		P.data[i] = data[i];
	return P;
}

template<class T, unsigned SIZE>
void normalize(Vec<T, SIZE>& v) {
  T t = length(v);
  if(t != 0.0f) {
    t = 1.0f / t;
    for(size_t i = 0; i < SIZE; ++i)
      v.data[i] *= t;
  }
}

template<class DATA_TYPE>
Vec<DATA_TYPE,3> cross(const Vec<DATA_TYPE, 3>& v1, const Vec<DATA_TYPE, 3>& v2 ) {
  Vec<DATA_TYPE,3> r;
  
  r.data[0] = (v1.data[1] * v2.data[2]) - (v1.data[2] * v2.data[1]);
  r.data[1] = (v1.data[2] * v2.data[0]) - (v1.data[0] * v2.data[2]);
  r.data[2] = (v1.data[0] * v2.data[1]) - (v1.data[1] * v2.data[0]);

  return r;
}

template<class DATA_TYPE, unsigned SIZE>
Vec<DATA_TYPE, SIZE> abs(const Vec<DATA_TYPE, SIZE>& v) {
	Vec<DATA_TYPE, SIZE> r;
	for(size_t i = 0; i < SIZE; i++)
		r.data[i] = std::abs(v.data[i]);
	return r;
}

template<class DATA_TYPE, unsigned SIZE>
bool operator<(const Vec<DATA_TYPE, SIZE>& v, const DATA_TYPE& d) {
	for(size_t i = 0; i < SIZE; i++) {
		if(v.data[i] >= d)
			return false;
	}
	return true;
}

template<class DATA_TYPE, unsigned SIZE>
bool operator>(const Vec<DATA_TYPE, SIZE>& v, const DATA_TYPE& d) {
	return !(v < d);
}

template<class DATA_TYPE, unsigned SIZE>
Vec<DATA_TYPE, SIZE> prod(const boost::numeric::ublas::matrix<DATA_TYPE>& M, const Vec<DATA_TYPE, SIZE>& v) {
	Vec<DATA_TYPE, SIZE> r;
	assert(M.size1() == SIZE && M.size2() == SIZE);
	
	for(unsigned i = 0; i < M.size1(); i++) {
		r[i] = (DATA_TYPE)0;
		for(unsigned j = 0; j < M.size2(); j++) {
			r[i] += M(i, j) * v[j];
		}
	}

	return r;
}

template<class T, unsigned SIZE>
std::ostream& operator<<(std::ostream& os, const Vec<T, SIZE>& v) {
  os << "(";
  for(size_t i = 0; i < SIZE; i++)
    os << v[i] << ", ";
  os << "\b\b)";
  return os;
}

typedef Vec<float, 2> Vec2f;
typedef Vec<double, 2> Vec2d;
typedef Vec<int, 2> Vec2i;
typedef Vec<int, 3> Vec3i;
typedef Vec<size_t, 2> Vec2ui;
typedef Vec<size_t, 3> Vec3ui;
typedef Vec<size_t, 4> Vec4ui;
typedef Vec<float, 1> Vec1f;
typedef Vec<float, 3> Vec3f;
typedef Vec<double, 3> Vec3d;
typedef Vec<float, 4> Vec4f;
typedef Vec<double, 4> Vec4d;

//inline std::ostream& operator<<(std::ostream& os, Vec2f& v) {
//  return os << "(" << v[0] <<", " << v[1] << ")";
//}

}

#endif //__VECTOR__
